﻿using System.Text;
using TPXSDK.NET.GIS;

namespace TPXSDK.NET.Mission.Instruction;

internal class InstCamCtrl : Tokenizer
{
    #region 成员变量
    private const byte ActCamStop = 0x00;
    private const byte ActCamStartByDist = 0x01;
    private const byte ActCamStartByTime = 0x02;
    private const byte ActCamRunByDist = 0x03;
    #endregion

    public InstCamCtrl(TokenizerAdmin admin)
    {
        Admin = admin;
        KeyName = "camctrl";
        KeyValue = (int)FlyCmdId.CMD_CAMCTRL;
        Usage = "camctrl [start|run|stop] [dist|time] [para]";
        Description = @"command to control camera by distance or time.";
    }

    public override bool Process()
    {
        var flyCmp = Admin?.Compiler; if (null == flyCmp) return false;
        var tokens = flyCmp.GetLineTokens();

        if (null == tokens || tokens.Count > 5 || tokens.Count < 2)
        {
            flyCmp.ErrorMsg = $"{KeyName} Invalid Usage";
            return false;
        }

        var cmd = new FlyCmdItem
        {
            Id = (byte)KeyValue,
            LineNum = tokens[0].Row
        };

        tokens.RemoveAt(0);// Remove Flycmd Name

        byte actCode = 0xFF;
        if (string.Equals(tokens[0].Name, "start", StringComparison.OrdinalIgnoreCase))
            actCode = ActCamStartByDist;
        if (string.Equals(tokens[0].Name, "stop", StringComparison.OrdinalIgnoreCase))
            actCode = ActCamStop;
        if (string.Equals(tokens[0].Name, "run", StringComparison.OrdinalIgnoreCase))
            actCode = ActCamRunByDist;
        if (actCode == 0xFF)
        {
            flyCmp.ErrorMsg = $"{KeyName} Invalid param";
            return false;
        }

        if (actCode is ActCamStartByDist or ActCamRunByDist)
        {
            if (tokens.Count == 1)
            {// 不带任何参数的情况， camctrl [start|run] (立即拍照)
                cmd.P1 = actCode;
                cmd.P2 = 0;
            }
            else if (tokens.Count == 3 || tokens.Count == 4)
            {
                if (tokens[2].Type != TokenType.NUMBER)
                {
                    return false;
                }

                var distMode = string.Equals(tokens[1].Name, "dist", StringComparison.OrdinalIgnoreCase);
                var timeMode = string.Equals(tokens[1].Name, "time", StringComparison.OrdinalIgnoreCase);

                if (distMode) cmd.P1 = actCode;
                if (timeMode) cmd.P1 = ActCamStartByTime;

                var paramVal = Convert.ToSingle(tokens[2].Name);
                if (distMode)
                {
                    var distCm = paramVal * 100.0f;
                    if (distCm < 100 || distCm > 32000)// 无效距离参数
                    {
                        flyCmp.ErrorMsg = $"{KeyName} Invalid dist param";
                        return false;
                    }
                    cmd.P2 = (short)distCm;
                }
                else
                {
                    cmd.P2 = (short)paramVal;
                }

                if (tokens.Count == 4)
                {// 带坐标位置参数
                    var pos = FlyUtils.Str2AbsPos(tokens[3].Name);
                    if (pos != AbsPos.Zero)
                    {
                        cmd.P3.S32Val = (int)(pos.Lng * 1e7);
                        cmd.P4.S32Val = (int)(pos.Lat * 1e7);
                    }
                    else
                    {
                        cmd.P3.S32Val = 0;
                        cmd.P4.S32Val = 0;
                    }
                }
            }
            else
            {
                return false;
            }
        }
        else if (ActCamStop == actCode)
        {// camctrl stop
            cmd.P1 = 0;
            cmd.P2 = 0;
            if (tokens.Count == 2) // camctrl stop 1000
            {
                if (tokens[1].Type == TokenType.NUMBER)
                {// 带超时参数
                    cmd.P2 = FlyUtils.ToInt16(tokens[1].Name);
                }
                else if (tokens[1].Type == TokenType.ABS_POS)
                {// 带坐标位置参数
                    cmd.P2 = 0;
                    var pos = FlyUtils.Str2AbsPos(tokens[1].Name);
                    if (pos != AbsPos.Zero)
                    {
                        cmd.P3.S32Val = (int)(pos.Lng * 1e7);
                        cmd.P4.S32Val = (int)(pos.Lat * 1e7);
                    }
                    else
                    {
                        cmd.P3.S32Val = 0;
                        cmd.P4.S32Val = 0;
                    }
                }
                else
                {
                    return false;
                }
            }
        }

        flyCmp.CmdList.Add(cmd);
        return true;
    }

    public override void Reverse(FlyCmdItem cmd, StringBuilder sb)
    {
        var mode = "???";
        if (cmd.P1 == ActCamStop)
        {
            if (0 != cmd.P3.S32Val && 0 != cmd.P4.S32Val)
            {
                var wp = FlyUtils.ParseWpFromCmdItem(cmd.P3, cmd.P4);
                sb.AppendLine($"{KeyName} stop {wp}");
            }
            else
            {
                sb.AppendLine(0 == cmd.P2 ? $"{KeyName} stop" : $"{KeyName} stop {cmd.P2}");
            }
            return;
        }

        if (cmd.P1 == ActCamStartByDist) mode = "dist";
        if (cmd.P1 == ActCamRunByDist) mode = "dist";
        if (cmd.P1 == ActCamStartByTime) mode = "time";
        var strAct = cmd.P1 == ActCamRunByDist ? "run" : "start";

        if (cmd.P2 == 0)
        {
            sb.AppendLine($"{KeyName} {strAct}");
        }
        else
        {
            if (cmd.P1 == ActCamStartByDist ||
                cmd.P1 == ActCamRunByDist)
            {
                var para2 = cmd.P2 * 0.01f;// cm->m
                if (0 == cmd.P3.S32Val || 0 == cmd.P4.S32Val)
                    sb.AppendLine($"{KeyName} {strAct} {mode} {para2:F2}");
                else
                {
                    var wp = FlyUtils.ParseWpFromCmdItem(cmd.P3, cmd.P4);
                    //if(cmd.P1 == ActCamRunByDist) wp = string.Empty;
                    sb.AppendLine($"{KeyName} {strAct} {mode} {para2:F2} {wp}");
                }
            }
            else
            {
                sb.AppendLine($"{KeyName} {strAct} {mode},{cmd.P2}");
            }
        }
    }

}
